/**
 * @file algo_Filter.c
 * @brief Software implementation of Signal Filter for 51 microcontroller.
 * @author David Xu
 * @version 1.0.0
 * @date 2023-11-21
 * @note none
 */

#include "algo_Filter.h"
#include <math.h>
#include "ct_head.h"
#include "algo_num.h"


//-------------------------------------
// 1. (a low-pass filter)
//-------------------------------------

void LPF1_Standard(int16_t *output, int16_t new_sample, uint8_t percentTimes256) {
    int32_t tmp_output;

    // Calculate the weighted contribution of the new sample
    int16_t new_sample_weighted = new_sample;

    // Calculate the weighted contribution of the current output
    int16_t output_weighted = Percent2Result(*output - new_sample, percentTimes256);

    // Combine the weighted contributions and update the output
    tmp_output = new_sample_weighted + output_weighted;
    *output    = tmp_output;
}

/*============================================================================*/

void LPF1_FP16(float *output, float new_sample, float percent) {
    qFP16_t sample_fp16  = qFP16_FloatToFP(new_sample);
    qFP16_t output_fp16  = qFP16_FloatToFP(*output);
    qFP16_t percent_fp16 = qFP16_FloatToFP(percent);
    qFP16_t tmp1_fp16, tmp2_fp16;

    // Calculate the weighted contribution of the new sample
    tmp1_fp16 = qFP16_Sub(output_fp16, sample_fp16);
    // Calculate the weighted contribution of the current output
    tmp2_fp16 = qFP16_Mul(percent_fp16, tmp1_fp16);
    *output   = qFP16_FPToFloat(qFP16_Add(sample_fp16, tmp2_fp16));
}

//-------------------------------------
// 1.1 (second-order low-pass filter)
//-------------------------------------

int init_LPF2_FP16(LPF2_State_t *state, float input_alpha) {
    qFP16_t alpha, one, two, aa, p1, r;
    qFP16_t fp_tmp1, fp_tmp2;

    if (state == NULL || input_alpha <= 0.0f || input_alpha >= 1.0f) {
        return -1;    // Return an error code to indicate invalid parameters
    }

    alpha = qFP16_FloatToFP(input_alpha);
    one   = qFP16_Constant(1.0f);
    two   = qFP16_Constant(2.0f);

    // Calculate aa = alpha * alpha
    aa = qFP16_Mul(alpha, alpha);

    // Calculate p1 = sqrt(2.0 * alpha)
    p1 = qFP16_Sqrt(qFP16_Mul(two, alpha));

    // Calculate r = 1.0 + p1 + aa
    r = qFP16_Add(qFP16_Add(one, p1), aa);

    // Calculate k = aa / r
    state->k = qFP16_Div(aa, r);

    // Calculate a1 = 2.0 * (aa - 1.0) / r
    fp_tmp1   = qFP16_Sub(aa, one);
    fp_tmp2   = qFP16_Mul(two, fp_tmp1);
    state->a1 = qFP16_Div(fp_tmp2, r);

    // Calculate a2 = (1.0 - p1 + aa) / r
    fp_tmp1   = qFP16_Sub(one, p1);
    fp_tmp2   = qFP16_Add(fp_tmp1, aa);
    state->a2 = qFP16_Div(fp_tmp2, r);

    // Calculate b1 = 2.0 * aa / r
    fp_tmp1   = qFP16_Mul(two, aa);
    state->b1 = qFP16_Div(fp_tmp1, r);

    // b2 = k (already calculated)
    state->b2 = state->k;

    // Initialize internal state variables
    state->x1 = 0;
    state->x2 = 0;
    state->y1 = 0;
    state->y2 = 0;

    // Set the initialization flag
    state->init = 1u;    // Set to 1 to indicate that the structure has been initialized

    return 0;    // Return 0 to indicate success
}

/*============================================================================*/

int16_t LPF2_FP16(LPF2_State_t *state, int16_t new_sample) {
    qFP16_t temp_output, mul_k_new_sample, mul_b1_x1, mul_b2_x2, mul_a1_y1, mul_a2_y2;
    qFP16_t add_b1_b2, sub_a1_a2;
    qFP16_t q_new_sample = qFP16_IntToFP(new_sample);

    // If it is the first call, initialize the state
    if (state->init) {
        state->y1   = q_new_sample;
        state->y2   = q_new_sample;
        state->x1   = q_new_sample;
        state->x2   = q_new_sample;
        state->init = 0u;
    }

    // Calculate intermediate values to avoid nested calculations
    mul_k_new_sample = qFP16_Mul(q_new_sample, state->k);
    mul_b1_x1        = qFP16_Mul(state->x1, state->b1);
    mul_b2_x2        = qFP16_Mul(state->x2, state->b2);
    mul_a1_y1        = qFP16_Mul(state->y1, state->a1);
    mul_a2_y2        = qFP16_Mul(state->y2, state->a2);

    // Continue with the calculations
    add_b1_b2 = qFP16_Add(mul_b1_x1, mul_b2_x2);
    sub_a1_a2 = qFP16_Sub(qFP16_Sub(qFP16_Constant(0), mul_a1_y1), mul_a2_y2);

    // Final output calculation
    temp_output = qFP16_Add(mul_k_new_sample, qFP16_Add(add_b1_b2, sub_a1_a2));

    // Update the state
    state->x2 = state->x1;
    state->x1 = q_new_sample;
    state->y2 = state->y1;
    state->y1 = temp_output;

    // Ensure the output is within the valid range and convert back to integer
    return qFP16_FPToInt(temp_output);
}


//-------------------------------------
// 2.1 Sliding Window Filter
//-------------------------------------
void init_SW_filter(SW_Filter_t *state, uint8_t shift) {
    state->sum   = 0;        // Initialize the cumulative sum to 0
    state->shift = shift;    // Set the shift parameter for the window size
    state->init  = 1;        // Initialize the flag
}

/*============================================================================*/

int16_t SW_filter(SW_Filter_t *state, int16_t new_value) {
    int16_t filtered_value;
    if (state->init) {
        state->sum  = new_value << state->shift;
        state->init = 0;
    }
    // Update the cumulative sum
    state->sum = state->sum + new_value - (state->sum >> state->shift);
    // Calculate and return the filtered value
    filtered_value = state->sum >> state->shift;
    return filtered_value;
}

//-------------------------------------
// 2.2 Moving average filter
//-------------------------------------

void init_moving_average(MW_Filter_t *state, int16_t *filterBuffer, uint16_t size, uint8_t shift) {
    state->buffer    = filterBuffer;    // Point to the static array
    state->sum       = 0;
    state->index     = 0;
    state->buf_size  = size;     // Set buffer size
    state->buf_shift = shift;    // Set shift amount
    state->init      = TRUE;     // Initialize flag to 1
}
/*============================================================================*/

int16_t moving_average_filter(MW_Filter_t *state, int16_t new_value) {
    int16_t i;
    // Initialize the current position of the buffer
    if (state->init) {    // Assuming initialization occurs at index 0
        for (i = 0; i < state->buf_size; i++) {
            state->buffer[i] = new_value;
        }
        state->sum  = new_value * state->buf_size;
        state->init = 0;
    } else {
        state->sum -= state->buffer[state->index];
        state->buffer[state->index] = new_value;    // Multiply input data by scaling factor
        state->sum += state->buffer[state->index];
    }
    state->index = (state->index + 1) & (state->buf_size - 1);
    return state->sum >> state->buf_shift;    // Returns the amplified average value
}


//-------------------------------------
// 2.3 Weighted moving average filter (reentrant version)
//-------------------------------------

void weighted_init_moving_average(weighted_MW_Filter_t *state, int16_t *filterBuffer, int16_t *weight,
                                  uint16_t size, uint8_t shift) {
    state->buffer    = filterBuffer;    // Point to the static array
    state->weight    = weight;          // Point to the weight factor array
    state->sum       = 0;
    state->index     = 0;
    state->buf_size  = size;     // Set buffer size
    state->buf_shift = shift;    // Set shift amount
    state->init      = 1;        // Initialize flag to 1
}

/*============================================================================*/

int16_t weighted_moving_average_filter(weighted_MW_Filter_t *state, int16_t new_value) {
    int16_t i;
    // Initialize the current position of the buffer
    if (state->init) {    // Assuming initialization occurs at index 0
        for (i = 0; i < state->buf_size; i++) {
            state->buffer[i] = new_value;
        }
        state->sum  = new_value * state->buf_size;
        state->init = 0;
    } else {
        state->sum -= Percent2Result(state->buffer[state->index], state->weight[state->index]);
        state->buffer[state->index] = new_value;    // Multiply input data by scaling factor
        state->sum += Percent2Result(state->buffer[state->index], state->weight[state->index]);
    }
    state->index = (state->index + 1) & (state->buf_size - 1);
    return state->sum >> state->buf_shift;    // Returns the amplified average value
}

//-------------------------------------
// 2.4 Moving Outlier Removal (MOR) Filter
//-------------------------------------

void init_mor_filter(MOR_Filter_t *state, int16_t *buffer, uint16_t buf_size, uint8_t shift, int16_t threshold) {
    uint8_t i;
    state->buffer    = buffer;
    state->buf_size  = buf_size;    // Set buffer size
    state->buf_shift = shift;       // Set shift amount
    state->median    = 0;
    state->threshold = threshold;
    state->sum       = 0;
    state->index     = 0;
    state->init      = true;
    // Initialize the buffer array
    for (i = 0; i < buf_size; ++i) {
        buffer[i] = 0;
    }
}

/*============================================================================*/

int16_t mor_filter_update(MOR_Filter_t *state, int16_t new_value) {
    uint8_t i;
    // If not initialized yet, fill the entire buffer with the new value
    if (state->init) {
        for (i = 0; i < state->buf_size; ++i) {
            state->buffer[i] = new_value;
        }
        state->median = new_value;
        state->sum    = new_value << state->buf_shift;
        state->init   = false;
        return new_value;
    }
    // Check if the new value is an outlier
    if (abs(new_value - state->median) > state->threshold) {
        // If it is an outlier, return the previous median
        return state->median;
    } else {
        // If it is not an outlier, update the buffer and median
        state->median = new_value;
        state->index  = (state->index + 1) % state->buf_size;
        state->sum -= state->buffer[state->index];
        state->buffer[state->index] = new_value;
        state->sum += state->buffer[state->index];
        return state->sum >> state->buf_shift;
        ;
    }
}

//-------------------------------------
//  3. Median Filter
//-------------------------------------

void init_MedianFilter(MedianFilte_t *state, int16_t *buffer, uint16_t size) {
    state->buffer     = buffer;    // Assign the address of the external array to the pointer
    state->next_index = 0;
    state->size       = size;
    state->init       = true;    // Initialization completed
}

/*============================================================================*/
int16_t MedianFilter(MedianFilte_t *state, int16_t new_sample) {
    int16_t  median_val;
    uint16_t i;

    // Check if the filter has been initialized
    if (state->init) {
        for (i = 0; i < state->size; i++) {
            state->buffer[i] = 0;
        }
        state->init = false;
    }

    // Add the new sample to the buffer and update the index
    state->buffer[state->next_index] = new_sample;
    state->next_index                = (state->next_index + 1) % state->size;

    // Sort the buffer in ascending order
    insertSortAscending(state->buffer, state->size);

    // Calculate the median value
    median_val = state->buffer[state->size >> 1];
    return median_val;
}

//-------------------------------------
// 4. Scalar Kalman Filter
//-------------------------------------
void KalmanFilter_Init(KalmanParams_t *const s, float initial_x, float initial_p, float A, float H,
                       float Q, float R) {
    s->x    = qFP16_FloatToFP(initial_x);    // Initial state estimate
    s->P    = qFP16_FloatToFP(initial_p);    // Initial error covariance
    s->A    = qFP16_FloatToFP(A);            // State transition matrix
    s->H    = qFP16_FloatToFP(H);            // Observation matrix
    s->Q    = qFP16_FloatToFP(Q);            // Process noise covariance
    s->R    = qFP16_FloatToFP(R);            // Measurement noise covariance
    s->gain = 0;                             // Initial Kalman gain set to 0
    s->init = 1u;                            // Set initialization flag to indicate the need for initialization
}

/*============================================================================*/

int16_t KalmanFilter(KalmanParams_t *const s, const int16_t measuredValue) {
    qFP16_t pH, temp1, temp2, temp3;
    qFP16_t measuredValueQFP16 = qFP16_IntToFP(measuredValue);
    //
    if (1u == s->init) {
        s->x    = measuredValueQFP16;
        s->init = 0u;
    }

    // Predict
    // s->x = s->A * s->x;
    temp1 = qFP16_Mul(s->A, s->x);

    // s->P = s->A * s->A * s->P + s->Q;
    temp2 = qFP16_Mul(s->A, s->A);
    temp3 = qFP16_Mul(temp2, s->P);
    s->P  = qFP16_Add(temp3, s->Q);

    // Measurement Update
    // pH = s->P * s->H;
    pH = qFP16_Mul(s->P, s->H);

    // s->gain = pH / (s->R + s->H * pH);
    temp1   = qFP16_Mul(s->H, pH);
    temp1   = qFP16_Add(s->R, temp1);
    s->gain = qFP16_Div(pH, temp1);

    // s->x = s->x + s->gain * (x - s->H * s->x);
    temp1 = qFP16_Mul(s->H, s->x);
    temp2 = qFP16_Sub(measuredValueQFP16, temp1);
    temp3 = qFP16_Mul(s->gain, temp2);
    s->x  = qFP16_Add(s->x, temp3);

    // s->P = (1 - s->gain * s->H) * s->P;
    temp1 = qFP16_Mul(s->gain, s->H);
    temp2 = qFP16_Sub(qFP16_Constant(1.0), temp1);
    s->P  = qFP16_Mul(temp2, s->P);

    return qFP16_FPToInt(s->x);
}

//-------------------------------------
// 5. Gaussian Filter
//-------------------------------------
int8_t init_GMWFilter(qSSmoother_GMWF_t *state, const qFP16_t *params, qFP16_t *window, uint16_t window_size) {

    qFP16_t  sigma, center, sum, r, exponent, distance;
    uint16_t half_window_size, i;

    if (window == NULL || window_size == 0) {
        return -1;    // Error
    }

    sigma            = params[0];    // Standard deviation represented in fixed-point format
    center           = params[1];    // Center point
    sum              = qFP16_IntToFP(0);
    r                = qFP16_Mul(qFP16_IntToFP(2), qFP16_Mul(sigma, sigma));
    half_window_size = window_size / 2;
    i                = 0;

    // Initialize window and kernel arrays
    state->window      = window;
    state->kernel      = window + half_window_size;    // Kernel array follows the window array
    state->window_size = half_window_size;
    state->init        = TRUE;

    for (i = 0; i < half_window_size; ++i) {
        distance         = qFP16_Sub(qFP16_IntToFP(i), qFP16_IntToFP(half_window_size));
        distance         = qFP16_Sub(distance, center);
        exponent         = qFP16_Div(qFP16_Mul(-distance, distance), r);
        state->kernel[i] = qFP16_Exp(exponent);    // Use fixed-point version of the exp function
        sum              = qFP16_Add(sum, state->kernel[i]);
    }

    // Normalize the kernel array
    for (i = 0; i < half_window_size; ++i) {
        state->kernel[i] = qFP16_Div(state->kernel[i], sum);
    }

    return 0;    // Success
}

/*============================================================================*/

int16_t GMWFilter(qSSmoother_GMWF_t *state, int16_t x) {
    uint16_t i;
    qFP16_t  y;
    if (state->init) {
        for (i = 0; i < state->window_size; ++i) {
            state->window[i] = qFP16_IntToFP(x);
        }
        state->init = FALSE;
    } else {
        for (i = state->window_size - 1; i > 0; --i) {
            state->window[i] = state->window[i - 1];
        }
    }
    state->window[0] = qFP16_IntToFP(x);

    y = qFP16_IntToFP(0);
    for (i = 0; i < state->window_size; ++i) {
        y = qFP16_Add(y, qFP16_Mul(state->window[i], state->kernel[i]));
    }

    return qFP16_FPToInt(y);
}

//-------------------------------------
// 6.1 Exponential Smoothing Filter
//-------------------------------------

int init_ExpFilter(ExpFilter_t *const s, const float lambda) {
    int retValue = 0;
    if ((lambda > 0) && (lambda < 1.0f)) {
        s->lambda = qFP16_FloatToFP(lambda);
        s->m      = 0;
        s->w      = qFP16_Constant(1.0f);
        s->init   = 1u;
        retValue  = 1u;
    }
    return retValue;
}

/*============================================================================*/

qFP16_t ExpFilter(ExpFilter_t *const s, const int16_t x) {
    qFP16_t iw;
    qFP16_t fp_tmp1, fp_tmp2, fp_tmp3;
    if (1u == s->init) {
        s->m    = qFP16_IntToFP(x);
        s->w    = qFP16_Constant(1.0f);
        s->init = 0u;
    }
    // Calculate w = lambda * w + 1.0f
    fp_tmp1 = qFP16_Mul(s->lambda, s->w);
    fp_tmp2 = qFP16_Add(fp_tmp1, qFP16_Constant(1.0f));
    s->w    = fp_tmp2;
    // Calculate iw = 1.0f / w
    iw = qFP16_Div(qFP16_Constant(1.0f), s->w);
    // Calculate m = m * (1.0f - iw) + iw * x
    fp_tmp1 = qFP16_Sub(qFP16_Constant(1.0f), iw);
    fp_tmp2 = qFP16_Mul(iw, qFP16_IntToFP(x));
    fp_tmp3 = qFP16_Mul(s->m, fp_tmp1);
    s->m    = qFP16_Add(fp_tmp3, fp_tmp2);
    return s->m;
}


//-------------------------------------
// 6.2 Double Exponential Smoothing Filter
//-------------------------------------

int16_t init_DoubleExpFilter(DESFilter_t *const s, const float *const param) {
    qFP16_t retValue = 0;
    qFP16_t a        = qFP16_FloatToFP(param[0]);
    qFP16_t b        = qFP16_FloatToFP(param[1]);
    qFP16_t n        = qFP16_FloatToFP(param[2]);
    if ((n >= qFP16_Constant(0.0f)) && (a > qFP16_Constant(0.0f)) && (a < qFP16_Constant(1.0f))
        && (b > qFP16_Constant(0.0f)) && (b < qFP16_Constant(1.0f))) {
        s->alpha = a;
        s->beta  = b;
        s->n     = qFP16_Round(n);
        s->init  = 1u;
        retValue = 1u;
    }
    return retValue;
}

/*============================================================================*/

int16_t DoubleExpFilter(DESFilter_t *const s, const int16_t x) {
    qFP16_t x_fp16 = qFP16_IntToFP(x);
    qFP16_t lt_1, level, trend, result;
    qFP16_t fp_tmp1, fp_tmp2, fp_tmp3;
    if (1 == s->init) {
        s->lt   = x_fp16;
        s->bt   = x_fp16;
        s->init = 0;
    }
    lt_1 = s->lt;
    // level = (1 - alpha) * lt_1 + alpha * x_fp16
    fp_tmp1 = qFP16_Sub(qFP16_Constant(1.0f), s->alpha);
    fp_tmp2 = qFP16_Mul(s->alpha, x_fp16);
    level   = qFP16_Add(qFP16_Mul(fp_tmp1, lt_1), fp_tmp2); /* level */
    s->lt   = level;
    // trend = (1 - beta) * bt + beta * (lt - lt_1)
    fp_tmp1 = qFP16_Sub(qFP16_Constant(1.0f), s->beta);
    fp_tmp2 = qFP16_Sub(s->lt, lt_1);
    fp_tmp3 = qFP16_Mul(s->beta, fp_tmp2);
    trend   = qFP16_Add(qFP16_Mul(fp_tmp1, s->bt), fp_tmp3); /* trend */
    s->bt   = trend;
    result  = qFP16_Add(level, qFP16_Mul(s->n, trend)); /* model/forecast */

    return qFP16_FPToInt(result);
}


#if 0


#define SAMPLES 128    // 定义样本数量
#define PI      3.14159265358979323846

// 测试样本生成函数，这里我们生成一个含有噪声的正弦波
void generateTestSamples(int16_t *samples, int numSamples) {
    int i;
    for (i = 0; i < numSamples; i++) {

        samples[i] = (int16_t)(1000);    // * sin(2 * PI * i / 10));

        samples[i] += (rand() % 150) - 50;
    }
}

// 测试LPF1_Simple函数
void test_LPF1_Simple() {
    int16_t samples[SAMPLES];
    int16_t filteredSamples[SAMPLES];
    int16_t output = 0;
    int     i;
    // 生成测试样本
    generateTestSamples(samples, SAMPLES);


    for (i = 0; i < SAMPLES; i++) {
        LPF1_Standard(&output, samples[i], 200);
        filteredSamples[i] = output;
    }

    // 输出结果，或者使用图形化工具绘制样本和滤波后的样本
    for (i = 0; i < SAMPLES; i++) {
        printf("Original: %d, Filtered: %d\n", samples[i], filteredSamples[i]);
    }
}

// 测试LPF2_FP16函数
void test_LPF2_FP16() {
    int16_t      samples[SAMPLES];
    int16_t      filteredSamples[SAMPLES];
    LPF2_State_t lpf2_state;
    int          i;
    // 初始化LPF2状态
    if (init_LPF2_FP16(&lpf2_state, 0.1f) != 0) {
        printf("LPF2 initialization failed.\n");
        return;
    }

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 应用LPF2_FP16滤波器
    for (i = 0; i < SAMPLES; i++) {
        filteredSamples[i] = LPF2_FP16(&lpf2_state, samples[i]);
    }

    // 输出结果，或者使用图形化工具绘制样本和滤波后的样本
    for (i = 0; i < SAMPLES; i++) {
        printf("Original: %d, Filtered: %d\n", samples[i], filteredSamples[i]);
    }
}
// 测试滑动窗口滤波器
void test_SW_filter() {
    int         i;
    int16_t     samples[SAMPLES];
    int16_t     filteredSamples[SAMPLES];
    SW_Filter_t sw_filter_state;

    // 初始化滑动窗口滤波器
    init_SW_filter(&sw_filter_state, 3);    // 假设窗口大小为2^3=8

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 应用滑动窗口滤波器
    for (i = 0; i < SAMPLES; i++) {
        filteredSamples[i] = SW_filter(&sw_filter_state, samples[i]);
    }

    // 输出结果，或者使用图形化工具绘制样本和滤波后的样本
    for (i = 0; i < SAMPLES; i++) {
        printf("Original: %d, Filtered: %d\n", samples[i], filteredSamples[i]);
    }
}

// 测试移动平均滤波器
void test_moving_average() {
    int         i;
    int16_t     samples[SAMPLES];
    int16_t     filteredSamples[SAMPLES];
    int16_t     filterBuffer[8];    // 假设窗口大小为8
    MW_Filter_t mw_filter_state;

    // 初始化移动平均滤波器
    init_moving_average(&mw_filter_state, filterBuffer, 8, 3);    // 假设窗口大小为2^3=8

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 应用移动平均滤波器
    for (i = 0; i < SAMPLES; i++) {
        filteredSamples[i] = moving_average_filter(&mw_filter_state, samples[i]);
    }

    // 输出结果，或者使用图形化工具绘制样本和滤波后的样本
    for (i = 0; i < SAMPLES; i++) {
        printf("Original: %d, Filtered: %d\n", samples[i], filteredSamples[i]);
    }
}

// 测试加权移动平均滤波器
// 测试加权移动平均滤波器
void test_weighted_moving_average() {
    int                  i;
    int16_t              samples[SAMPLES];
    int16_t              filteredSamples[SAMPLES];
    int16_t              filterBuffer[8];                                      // 假设窗口大小为8
    int16_t              weight[8] = { 16, 32, 64, 128, 255, 128, 64, 32 };    // 初始化权重数组，这里使用等权重
    weighted_MW_Filter_t weighted_mw_filter_state;

    // 初始化加权移动平均滤波器
    weighted_init_moving_average(&weighted_mw_filter_state, filterBuffer, weight, 8, 3);    // 假设窗口大小为2^3=8

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 应用加权移动平均滤波器
    for (i = 0; i < SAMPLES; i++) {
        filteredSamples[i] = weighted_moving_average_filter(&weighted_mw_filter_state, samples[i]);
    }

    // 输出结果，或者使用图形化工具绘制样本和滤波后的样本
    for (i = 0; i < SAMPLES; i++) {
        printf("Original: %d, Filtered: %d\n", samples[i], filteredSamples[i]);
    }
}

// 测试移动异常值移除滤波器
void test_mor_filter() {
    int16_t      mor_buffer[16];    // 窗口大小为8
    MOR_Filter_t mor_state;
    int16_t      samples[SAMPLES];
    int16_t      filtered_value;
    int          i;

    // 初始化MOR滤波器
    init_mor_filter(&mor_state, mor_buffer, 16, 4, 70);    // 窗口大小为8，位移为3

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新MOR滤波器并打印结果
    printf("MOR Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        filtered_value = mor_filter_update(&mor_state, samples[i]);
        // printf("%d\n", filtered_value);
        printf("Original: %d, Filtered: %d\n", samples[i], filtered_value);
    }
}

// 测试中值滤波器的例程
void test_median_filter() {
    int16_t       median_buffer[16];    // 窗口大小为8
    MedianFilte_t median_state;
    int16_t       samples[SAMPLES];
    int16_t       filtered_value;
    int           i;

    // 初始化中值滤波器
    init_MedianFilter(&median_state, median_buffer, 16);    // 窗口大小为8

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新中值滤波器并打印结果
    printf("Median Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        filtered_value = MedianFilter(&median_state, samples[i]);
        // printf("%d\n", filtered_value);
        printf("Original: %d, Filtered: %d\n", samples[i], filtered_value);
    }
}

// 测试标量卡尔曼滤波器的例程
void test_kalman_filter() {
    KalmanParams_t kalmanParams;
    int16_t        samples[SAMPLES];
    int16_t        estimated_value;
    int            i;

    // 初始化卡尔曼滤波器
    KalmanFilter_Init(&kalmanParams, 0.0f, 1.0f, 1.0f, 1.0f, 0.01f, 0.1f);

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新卡尔曼滤波器并打印结果
    printf("Kalman Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        estimated_value = KalmanFilter(&kalmanParams, samples[i]);
        // printf("%d\n", estimated_value);
        printf("Original: %d, Filtered: %d\n", samples[i], estimated_value);
    }
}



// 测试高斯移动窗口滤波器的例程
void test_gmw_filter() {
    qSSmoother_GMWF_t gmwFilterState;
    qFP16_t           gmwWindow[8];    // 窗口大小为8
    qFP16_t           gmwKernel[8];    // 假设高斯核已经在其他地方计算好了
    int16_t           samples[SAMPLES];
    int16_t           filtered_value;
    int               i;

    // 初始化高斯移动窗口滤波器
    init_GMWFilter(&gmwFilterState, gmwKernel, gmwWindow, 8);

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新高斯移动窗口滤波器并打印结果
    printf("Gaussian Moving Window Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        filtered_value = GMWFilter(&gmwFilterState, samples[i]);
        printf("Original: %d, Filtered: %d\n", samples[i], filtered_value);
    }
}

// 测试指数滤波器的例程
void test_exp_filter() {
    ExpFilter_t expFilter;
    int16_t     samples[SAMPLES];
    qFP16_t     filtered_value;
    int         i;

    // 初始化指数滤波器
    if (init_ExpFilter(&expFilter, 0.5f) == 0) {
        printf("Failed to initialize exponential filter.\n");
        return;
    }

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新指数滤波器并打印结果
    printf("Exponential Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        filtered_value = ExpFilter(&expFilter, samples[i]);
        // printf("%d\n", filtered_value);
        printf("Original: %d, Filtered: %d\n", samples[i], filtered_value);
    }
}


// 测试双指数平滑滤波器的例程
void test_des_filter() {
    DESFilter_t desFilter;
    int16_t     samples[SAMPLES];
    int16_t     filtered_value;
    float       params[3] = { 0.5f, 0.3f, 1.0f };    // alpha, beta, n
    int         i;

    // 初始化双指数平滑滤波器
    if (init_DoubleExpFilter(&desFilter, params) == 0) {
        printf("Failed to initialize double exponential smoothing filter.\n");
        return;
    }

    // 生成测试样本
    generateTestSamples(samples, SAMPLES);

    // 更新双指数平滑滤波器并打印结果
    printf("Double Exponential Smoothing Filter Output:\n");
    for (i = 0; i < SAMPLES; i++) {
        filtered_value = DoubleExpFilter(&desFilter, samples[i]);
        // printf("%d\n", filtered_value);
        printf("Original: %d, Filtered: %d\n", samples[i], filtered_value);
    }
}


void main(void) {
    gpio_init();
    timer_init();
    register_InterruptHander();
    ct_uart_Init(&g_Uart0, 115200);
    ct_uart_Init(&g_Uart1, 115200);
    // ??LPF1_Simple��?
    test_LPF1_Simple();

    // ??LPF2_FP16��?
    test_LPF2_FP16();
    //
    // 测试滑动窗口滤波器
    test_SW_filter();

    // 测试移动平均滤波器
    test_moving_average();

    // 测试加权移动平均滤波器
    test_weighted_moving_average();

    // 测试移动异常值移除滤波器
    test_mor_filter();
    // 测试中值滤波器的例程
    test_median_filter();

    // 测试标量卡尔曼滤波器
    test_kalman_filter();

    // 测试高斯移动窗口滤波器
    test_gmw_filter();

    // 测试指数滤波器
    test_exp_filter();

    // 测试双指数平滑滤波器
    test_des_filter();

    nop;

    while (1) {

        nop;
    }
}

#endif